#ifndef RAY_TRACE_PRO_KD_TREE_H
#define RAY_TRACE_PRO_KD_TREE_H

#include "stdafx.h"
#include "Windows/GxThreads.h"
#include "RayTraceSystem.h"
#include "Debug.h"
#include "Ray.h"
#include "BBox.h"
#include "DeviceList.h"
//#define DBG_KD_TRAVERSE_STATS

using namespace GxLibBasic;

namespace RayTracePro
{
	inline bool RayBBoxIntersection(const BBox & bbox, const Ray& ray, float & tmin, float & tmax)
	{
		float tymin, tymax, tzmin, tzmax;
		if (ray.ReciprocalDirection.x >= 0) 
		{
			tmin = (bbox.Min.x - ray.Origin.x) * ray.ReciprocalDirection.x;
			tmax = (bbox.Max.x - ray.Origin.x) * ray.ReciprocalDirection.x;
		}
		else
		{
			tmin = (bbox.Max.x - ray.Origin.x) * ray.ReciprocalDirection.x;
			tmax = (bbox.Min.x - ray.Origin.x) * ray.ReciprocalDirection.x;
		}
		if (ray.ReciprocalDirection.y >= 0) 
		{
			tymin = (bbox.Min.y - ray.Origin.y) * ray.ReciprocalDirection.y;
			tymax = (bbox.Max.y - ray.Origin.y) * ray.ReciprocalDirection.y;
		}
		else
		{
			tymin = (bbox.Max.y - ray.Origin.y) * ray.ReciprocalDirection.y;
			tymax = (bbox.Min.y - ray.Origin.y) * ray.ReciprocalDirection.y;
		}
		if ( tmin - tymax > Epsilon || tymin - tmax > Epsilon)
			return false;
		if (tymin > tmin)
			tmin = tymin;
		if (tymax < tmax)
			tmax = tymax;
		if (ray.ReciprocalDirection.z >= 0) 
		{
			tzmin = (bbox.Min.z - ray.Origin.z) * ray.ReciprocalDirection.z;
			tzmax = (bbox.Max.z - ray.Origin.z) * ray.ReciprocalDirection.z;
		}
		else
		{
			tzmin = (bbox.Max.z - ray.Origin.z) * ray.ReciprocalDirection.z;
			tzmax = (bbox.Min.z - ray.Origin.z) * ray.ReciprocalDirection.z;
		}
		if ( tmin - tzmax > Epsilon || tzmin - tmax > Epsilon )
			return false;
		if (tzmin > tmin)
			tmin = tzmin;
		if (tzmax < tmax)
			tmax = tzmax;
		tmax = GxLibMath::Min(ray.tMax, tmax);
		return tmin <= tmax;
	}

	inline void RayBBoxExit(const BBox & bbox, const Ray& ray, float tmin, float & tmax, int & face)
	{
		float tymax, tzmax;
		int bufFace;
		if (ray.ReciprocalDirection.x >= 0) 
		{
			tmax = (bbox.Max.x - ray.Origin.x) * ray.ReciprocalDirection.x;
			face = 3;
		}
		else
		{
			tmax = (bbox.Min.x - ray.Origin.x) * ray.ReciprocalDirection.x;
			face = 0;
		}
		if (ray.ReciprocalDirection.y >= 0) 
		{
			tymax = (bbox.Max.y - ray.Origin.y) * ray.ReciprocalDirection.y;
			bufFace = 4;
		}
		else
		{
			tymax = (bbox.Min.y - ray.Origin.y) * ray.ReciprocalDirection.y;
			bufFace = 1;
		}
		if (tymax > tmin && tymax < tmax)
		{
			tmax = tymax;
			face = bufFace;
		}
		if (ray.ReciprocalDirection.z >= 0) 
		{
			tzmax = (bbox.Max.z - ray.Origin.z) * ray.ReciprocalDirection.z;
			bufFace = 5;
		}
		else
		{
			tzmax = (bbox.Min.z - ray.Origin.z) * ray.ReciprocalDirection.z;
			bufFace = 2;
		}
		if (tzmax > tmin && tzmax < tmax)
		{
			tmax = tzmax;
			face = bufFace;
		}
		tmax = GxLibMath::Min(ray.tMax, tmax);
	}
	
	const int TraverseStackLength = 64;
	
	class Geometry;

	class KdNode
	{	
	private:
		unsigned int flag;
	public:
		inline unsigned int GetIsLeaf()
		{
			return flag & (1<<31);
		}
		inline unsigned int GetSplitDimension()
		{
			return flag & 3;
		}
		inline unsigned int GetOffset()
		{
			return flag & 0x7FFFFFFC;
		}
		inline unsigned int GetElementCount()
		{
			return flag & 0x7FFFFFFF;
		}
		inline void SetElementCount(int val)
		{
			flag = val + (flag & 0x80000000);
		}
		inline void SetIsLeaf(int val)
		{
			flag = (val<<31) + (flag & 0x7FFFFFFF);
		}
		inline void SetSplitDimension(int dim)
		{
			flag = dim + (flag & 0xFFFFFFFC);
		}
		inline void SetOffset(int val)
		{
			flag = val + (flag & 0x80000003);
		}
		union
		{
			float SplitPlane;
			int LeafPtr;
		};
	};

	class KdLeafHeader // 48 bytes
	{
	public:
		BBox Bounds;
		int Ropes[6];
		// Followed by Primitive References
		static const int FaceNegX = 0;
		static const int FaceNegY = 1;
		static const int FaceNegZ = 2;
		static const int FacePosX = 3;
		static const int FacePosY = 4;
		static const int FacePosZ = 5;
	};

	template <typename T>
	class KdTree
	{
	private:
		bool textureBuilt;
	public:
		int NodesTextureYMask, NodesTextureXMask;
		cl::Image2D NodesTexture;
		DeviceList<T> Elements;
		DeviceList<KdNode> Nodes;
		DeviceList<int> Leaves; // Raw memory; Layout: [KdLeafHeader(48), PrimitiveReference(4)*]*
		BBox Bounds;

		KdTree()
			: Elements(DeviceMemoryAccess::ReadOnly), 
			  Nodes(DeviceMemoryAccess::ReadOnly), 
			  Leaves(DeviceMemoryAccess::ReadOnly)
		{
			textureBuilt = false;
		}

		void ReOrderAsBFS()
		{
			DeviceList<KdNode> sortedNodes;
			sortedNodes.SetCapacity(Nodes.Count());
			Array<int, true> nodeMapping;
			nodeMapping.SetSize(Nodes.Count());
			Array<int, true> q1, q2;
			q1.SetCapacity(Nodes.Count()>>2);
			q1.Add(0);
			q2.SetCapacity(Nodes.Count()>>2);
			Array<int, true> * curQueue, * backQueue;
			curQueue = &q1;
			backQueue = &q2;
			while(curQueue->Count())
			{
				for (int i = 0; i<curQueue->Count(); i++)
				{
					int nodeId = curQueue->GetItem(i);
					auto node = Nodes[nodeId];
					if (!node.GetIsLeaf())
					{
						backQueue->Add(nodeId + 1);
						backQueue->Add(node.GetOffset()>>3);
					}
					sortedNodes.Add(node);
					nodeMapping[nodeId] = sortedNodes.Count()-1;
				}
				curQueue->Clear();
				auto tmp = curQueue;
				curQueue = backQueue;
				backQueue = tmp;
			}
			for (int i = 0; i<Nodes.Count(); i++)
			{
				if (!(Nodes[i].GetIsLeaf()))
				{
					int newNodeId = nodeMapping[i];
					sortedNodes[newNodeId].SetOffset(nodeMapping[i+1]<<3);
				}
				else
				{
					KdLeafHeader * leaf = (KdLeafHeader *)(Leaves.Buffer() + Nodes[i].LeafPtr/4 + Nodes[i].GetElementCount());
					for (int j = 0; j<6; j++)
					{
						if (leaf->Ropes[j] != -1)
							leaf->Ropes[j] = (nodeMapping[(leaf->Ropes[j]>>3)]<<3);
					}
				}
			}
			Nodes.Clear();
			Nodes.Add(sortedNodes);
		}

		void PrepareTextures()
		{
			if (!textureBuilt)
			{
				textureBuilt = true;
				int order = (int)ceil(log(sqrt((float)Nodes.Count()))/log(2.0f));
				int width = (1<<order);
				int height = (int)ceil(Nodes.Count()/(float)width);
				Nodes.SetCapacity(width * height);
				NodesTexture = cl::Image2D(GpuDevice::GetContext(), CL_MEM_COPY_HOST_PTR|CL_MEM_READ_ONLY, cl::ImageFormat(CL_RG, CL_UNSIGNED_INT32), 
					width, height, 0, Nodes.Buffer());
				NodesTextureXMask = width-1;
				NodesTextureYMask = order;
				//Nodes.Clear();
				//Nodes.Compress();
			}
		}
	};

	struct BBoxEdge
	{
		unsigned int Type : 1; // Start or End
		unsigned int GeometryId : 31;
		float t;
		static const int Start = 0;
		static const int End = 1;
		inline bool operator < (const BBoxEdge & e)
		{
			if (t == e.t)
				return Type<e.Type;
			else
				return t<e.t;
		}
	};

	template<typename T>
	inline void CreateLeafNode(KdTree<T> & tree, KdNode & curNode, int geomCount, int * geoms)
	{
		curNode.SetIsLeaf(1);
		curNode.SetElementCount(geomCount);
		curNode.LeafPtr = tree.Leaves.Count() * sizeof(int); // pointer
		// geometry references
		for (int g = 0; g<geomCount; g++)
			tree.Leaves.Add(geoms[g]);
		// paddings for tailer
		for (int i = 0; i<sizeof(KdLeafHeader)/sizeof(int); i++)
			tree.Leaves.Add(0);
	}

	template<typename T>
	void OptimizeRopes(KdTree<T> & tree, KdNode * node, int & rope, int face, BBox & box)
	{
		if (rope == -1)
			return;
		int faceAxis = face % 3;
		KdNode * nRope = (KdNode *)((char*)tree.Nodes.Buffer() + rope);
		while (!nRope->GetIsLeaf())
		{
			int splitAxis = nRope->GetSplitDimension();
			if (splitAxis == faceAxis)
			{
				if (face < 3)
					nRope = (KdNode *)((char*)tree.Nodes.Buffer() + nRope->GetOffset());
				else
					nRope = nRope + 1;
			}
			else if (nRope->SplitPlane < box.Min[splitAxis])
			{
				nRope = (KdNode *)((char*)tree.Nodes.Buffer() + nRope->GetOffset());
			}
			else if (nRope->SplitPlane > box.Max[splitAxis])
			{
				nRope = nRope + 1;
			}
			else
			{
				break;
			}
		}
		rope = (char*)nRope - (char*)tree.Nodes.Buffer();
	}

	template<typename T>
	void BuildRopes(KdTree<T> & tree, KdNode * node, int ropes[6], BBox & box)
	{
		if (!node->GetIsLeaf())
		{
			for (int i = 0; i<6; i++)
				OptimizeRopes(tree, node, ropes[i], i, box);
			int axis = (int)node->GetSplitDimension();
			BBox leftBox = box;
			BBox rightBox = box;
			leftBox.Max[axis] = rightBox.Min[axis] = node->SplitPlane;
			int leftRopes[6], rightRopes[6];
			for (int i = 0; i<6; i++)
			{
				leftRopes[i] = rightRopes[i] = ropes[i];
			}
			leftRopes[axis+3] = node->GetOffset();
			rightRopes[axis] = ((char*)(node+1)-(char*)tree.Nodes.Buffer());
			BuildRopes(tree, node+1, leftRopes, leftBox);
			BuildRopes(tree, (KdNode*)((char*)tree.Nodes.Buffer()+node->GetOffset()), rightRopes, rightBox);
		}
		else
		{
			int nodeId = node - tree.Nodes.Buffer();

			KdLeafHeader * header = (KdLeafHeader*)((char*)tree.Leaves.Buffer() + node->LeafPtr + node->GetElementCount()*sizeof(int));
			for (int i = 0; i<6; i++)
				header->Ropes[i] = ropes[i];
			header->Bounds = box;
		}
	}

	template<typename T, typename CostEvaluator>
	void ConstructKdTree(KdTree<T> & tree, T* elements, int nodeId, Array<BBox, true> & bboxes,
		BBoxEdge * edges, int * geoms, int geomCount, int * geomLeft, int * geomRight, int depth, int badRefines)
	{
		tree.Elements.SetSize(geomCount);
		for (int i = 0; i < geomCount; i++)
		{
			tree.Elements[i] = elements[i];
		}
		if (bboxes.Count() > 0)
		{
			tree.Bounds = bboxes[0];
			for (int i = 1; i < bboxes.Count(); i++)
			{
				tree.Bounds.Union(bboxes[i]);
			}
		}
		
		ConstructKdTreeRec<T, CostEvaluator>(tree, nodeId, bboxes, tree.Bounds, edges, geoms,
			geomCount, geomLeft, geomRight, depth, badRefines);
		printf("%d nodes\n", tree.Nodes.Count());
		
		int ropes[6] = {-1, -1, -1, -1, -1, -1};
		BuildRopes(tree, tree.Nodes.Buffer(), ropes, tree.Bounds);
		tree.ReOrderAsBFS();
		
	}

	template<typename T, typename CostEvaluator>
	void ConstructKdTreeRec(KdTree<T> & tree, int nodeId, Array<BBox, true> & bboxes, const BBox & nodeBBox,
		BBoxEdge * edges, int * geoms, int geomCount, int * geomLeft, int * geomRight, int depth, int badRefines)
	{
		tree.Nodes.Add(KdNode());
		#define CUR_NODE tree.Nodes[nodeId]
		if (geomCount <= CostEvaluator::ElementsPerNode || depth == 0)
		{
			CreateLeafNode(tree, CUR_NODE, geomCount, geoms);
			return;
		}
		int axis = 0;
		// Select split axis
		Vec3 nodeSize; Vec3::Subtract(nodeSize, nodeBBox.Max, nodeBBox.Min);
		if (nodeSize.y > nodeSize.x && nodeSize.y > nodeSize.z) axis = 1;
		else if (nodeSize.z > nodeSize.y && nodeSize.z > nodeSize.x) axis = 2;
		int otherAxis[3][2] = {{1, 2}, {0, 2}, {0, 1}};
		int axisRetry = 0;
selectOtherAxis:
		int axis0 = otherAxis[axis][0];
		int axis1 = otherAxis[axis][1];
		// Initialize geometry edges
		int candidates = 0;
		float boundsMin = nodeBBox.Min[axis];
		float boundsMax = nodeBBox.Max[axis];
		for (int i = 0; i<geomCount; i++)
		{
			int geomId = geoms[i];
			edges[i*2].GeometryId = edges[i*2+1].GeometryId = geomId;
			edges[i*2].t = bboxes[geomId].Min[axis]; edges[i*2].Type = BBoxEdge::Start;
			edges[i*2+1].t = bboxes[geomId].Max[axis]; edges[i*2+1].Type = BBoxEdge::End;
			if (edges[i*2].t > boundsMin && edges[i*2].t < boundsMax)
			{
				candidates ++;
			}
			if (edges[i*2+1].t > boundsMin && edges[i*2+1].t < boundsMax)
			{
				candidates ++;
			}
		}
		if (candidates == 0 && axisRetry < 3)
		{
			axis = (axis + 1)% 3;
			axisRetry ++;
			goto selectOtherAxis;
		}
		// Sort edges
		std::sort(edges, edges+geomCount*2);
		// Compute minimum cost split
		float sideArea2 = nodeSize[axis0] * nodeSize[axis1] * 2.0f;
		float invNodeArea = 1.0f/(sideArea2 + 2.0f *nodeSize[axis]* (nodeSize[axis0]+nodeSize[axis1]));
		float noSplitCost = CostEvaluator::IntersectTime * geomCount;
		float minCost = FLT_MAX;
		int bestSplit = -1;
		int nLeft = 0, nRight = geomCount;
		
		for (int i = 0; i<geomCount*2; i++)
		{
			BBoxEdge &thisEdge = edges[i];
			if (thisEdge.Type == BBoxEdge::End)
				nRight --;
			if (thisEdge.t > boundsMin && thisEdge.t < boundsMax)
			{
				float l1 = thisEdge.t - boundsMin;
				float l2 = boundsMax - thisEdge.t;
				float pLeft = (sideArea2 + 2.0f*l1*(nodeSize[axis0]+nodeSize[axis1]))*invNodeArea;
				float pRight = (sideArea2 + 2.0f*l2*(nodeSize[axis0]+nodeSize[axis1]))*invNodeArea;
				float be = (nLeft==0||nRight==0)?CostEvaluator::EmptyNodeBounus:0.0f;
				float cost = CostEvaluator::TraversalTime + (1.0f-be)*CostEvaluator::IntersectTime*(nLeft*pLeft+nRight*pRight);
				if (cost < minCost)
				{
					minCost = cost;
					bestSplit = i;
				}
			}
			if (thisEdge.Type == BBoxEdge::Start)
				nLeft ++;
		}
		if (minCost > noSplitCost && geomCount < 64 * CostEvaluator::ElementsPerNode)
			badRefines ++;
		
		if ((minCost > 4.0f * noSplitCost && geomCount < CostEvaluator::ElementsPerNode) || bestSplit == -1
			|| badRefines == 3 || (nLeft == nRight && nRight == geomCount))
		{
			CreateLeafNode(tree, CUR_NODE, geomCount, geoms);
			return;
		}
		// Classify elements
		float splitT = edges[bestSplit].t;
		nLeft = nRight = 0;
		int rightStart = 0;
		for (int i = 0; i<bestSplit; i++)
		{
			if (edges[i].Type == BBoxEdge::Start)
				geomLeft[nLeft++] = edges[i].GeometryId;
		}
		for (int i = bestSplit+1; i<geomCount*2; i++)
		{
			if (edges[i].Type == BBoxEdge::End)
				geomRight[nRight++] = edges[i].GeometryId;
		}
		/*for (int i = 0; i<geomCount*2; i++)
		{
			if (edges[i].Type == BBoxEdge::Start)
			{
				if (edges[i].t < splitT)
					rightStart = i + 1;
				if (edges[i].t <= splitT)
					geomLeft[nLeft++] = edges[i].GeometryId;
				else
					break;
			}
			
		}
		for (int i = rightStart; i<geomCount*2; i++)
		{
			if (edges[i].Type == BBoxEdge::End)
				geomRight[nRight++] = edges[i].GeometryId;
		}*/
		// Recursive split
		BBox subBBox = nodeBBox;
		CUR_NODE.SetIsLeaf(0);
		CUR_NODE.SetSplitDimension(axis);
		CUR_NODE.SplitPlane = splitT;
		float oriMax = subBBox.Max[axis];
		subBBox.Max[axis] = splitT;
		ConstructKdTreeRec<T, CostEvaluator>(tree, nodeId+1, bboxes, subBBox, edges, geomLeft, nLeft, geomLeft, geomRight+geomCount, depth-1, badRefines);
		CUR_NODE.SetOffset(tree.Nodes.Count() * sizeof(KdNode)); // right child id
		subBBox.Max[axis] = oriMax;
		subBBox.Min[axis] = splitT;
		ConstructKdTreeRec<T, CostEvaluator>(tree, tree.Nodes.Count(), bboxes, subBBox, edges, geomRight, nRight, geomLeft, geomRight+geomCount, depth-1, badRefines);
	}

	template<typename T>
	struct KdTraverseItem
	{
		KdNode * node;
		float tMin, tMax;
	};

	// delegate bool IterateAction(DifferentialGeometry & inter, T * elem, const Ray & ray, const RayDifferential & rayDiffIn, RayDifferential & diffOut, float & t)

	template<typename T>
	KdNode * LocateNode(KdTree<T> & tree, const Vec3 & p)
	{
		KdNode * rs = tree.Nodes.Buffer();
		while (!rs->GetIsLeaf())
		{
			int axis = rs->GetSplitDimension();
			if (p[axis] <= rs->SplitPlane)
				rs = (KdNode*)((char*)tree.Nodes.Buffer() + rs->GetOffset());
			else
				rs = (KdNode*)((char*)tree.Nodes.Buffer() + rs->GetOffset()) + 1;
		}
		return rs;
	}

	template<typename T, typename Tracer, bool pred, bool useMailbox>
	bool TraverseKdTree(RayTraceSystem * system, const Tracer & tracer, 
		DifferentialGeometry & rs, KdTree<T> & tree, const Ray & ray, const RayDifferential & rayDiff, float & t)
	{
#ifdef DBG_KD_TRAVERSE_STATS
		int nodeTraversed = 0;
		int leafTraversed = 0;
		int emptyLeafTraversed = 0;
		int triangleTraversed = 0;
#endif
		static volatile long rayIdAllocator = 1;
		float tmin, tmax;
		float normalDot = 10000.0f;
		if (!RayBBoxIntersection(tree.Bounds, ray, tmin, tmax))
			return false;
		tmax += Epsilon;
		tmin -= Epsilon;
		bool retVal = false;
		KdTraverseItem<T> stack[TraverseStackLength];
		KdNode * node = tree.Nodes.Buffer();
		Ray traceRay = ray;
		int stackPtr = 0;
		traceRay.tMax = tmax;
		rs.Normal = traceRay.Direction;
		rs.Position.SetZero();
		unsigned int rayId = 1;
		if (useMailbox)
			rayId = _InterlockedExchangeAdd(&rayIdAllocator, 1);	
		while (node)
		{
			if (traceRay.tMax < tmin)
				break;
			if (tmin > tmax)
				break;
#ifdef DBG_KD_TRAVERSE_STATS
			nodeTraversed ++;
#endif
			if (node->GetIsLeaf())
			{
				DifferentialGeometry inter;
				int count = node->GetElementCount();
#ifdef DBG_KD_TRAVERSE_STATS
				leafTraversed ++;
				triangleTraversed += count;
				if (count == 0)
					emptyLeafTraversed ++;
#endif
				int* elemIds = (int*)((char*)tree.Leaves.Buffer() + node->LeafPtr);
				for (int i = 0; i < count; i++)
				{
					T*curElem = tree.Elements.Buffer() + elemIds[i];
					float it = -1.0f;
					if (tracer.Trace(inter, *curElem, elemIds[i], traceRay, rayDiff, it))
					{
						if (pred)
							return true;		
						//float ndot = 0.0f;
						if (it <= traceRay.tMax/* - Epsilon || 
							(it < traceRay.tMax + Epsilon && 
							(ndot = Vec3::Dot(inter.Normal, traceRay.Direction)) < normalDot)*/)
						{
							//normalDot = ndot;
							rs = inter;
							traceRay.tMax = it;
							t = it;
							retVal = true;
						}
					}
				}
				if (stackPtr)
				{
					KdTraverseItem<T> & item = stack[--stackPtr];
					node = item.node;
					tmax = item.tMax;
					tmin = item.tMin;
				}
				else
					break;
			}
			else // Interior node
			{
				int axis = (int)(node->GetSplitDimension());
				//bool coplanar = abs(ray.Direction[axis]) < Epsilon;
				float tplane;
				//if (!coplanar)
				tplane = (node->SplitPlane - ray.Origin[axis]) * ray.ReciprocalDirection[axis];
				KdNode* node1, *node2;
				float rayOrigin = ray.Origin[axis];
				if (rayOrigin <= node->SplitPlane)
				{
					node1 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());//node + 1;
					node2 = node1 + 1; //(KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());
				}
				else
				{
					node1 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset()) + 1;
					node2 = node1 - 1;
				}

				if (/*coplanar||*/(tplane > tmax || tplane < 0))
				{
					node = node1;
				}
				else if (tplane < tmin)
				{
					node = node2;
				}
				else
				{
					KdTraverseItem<T> toTraverse;
					toTraverse.node = node2;
					toTraverse.tMin = tplane;
					toTraverse.tMax = tmax;
					stack[stackPtr++] = toTraverse;
					node = node1;
					tmax = tplane;
				}
			}
		}
#ifdef DBG_KD_TRAVERSE_STATS
		if (system->DebugPrint)
		{
			printf("Nodes traversed: %d\n", nodeTraversed);
			printf("leaves traversed: %d\n", leafTraversed);
			printf("empty leaf traversed: %d\n", emptyLeafTraversed);
			printf("triangles traversed: %d\n", triangleTraversed);
		}
#endif
		
		return retVal;
	}

	template<typename T, typename Tracer, bool pred, bool useMailbox>
	bool TraverseKdTreeStackless(RayTraceSystem * system, KdNode * root, const Tracer & tracer, 
		DifferentialGeometry & rs, KdTree<T> & tree, const Ray & ray, const RayDifferential & rayDiff, float & t)
	{
		float sceneMin, sceneMax;
		if (!RayBBoxIntersection(tree.Bounds, ray, sceneMin, sceneMax))
			return false;
		sceneMin -= system->GeometryBias;
		sceneMax += system->GeometryBias;
		Ray traceRay = ray;
		traceRay.tMax = sceneMax;
		rs.Normal = traceRay.Direction;
		rs.Position.SetZero();
		bool intersected = false;
		float tMin = sceneMin;
		float tMax = sceneMax;
		KdNode * node = root;
		while (tMin < tMax)
		{
			while (!node->GetIsLeaf())
			{
				int axis = (int)(node->GetSplitDimension());
				float tplane;
				tplane = (node->SplitPlane - ray.Origin[axis]) * ray.ReciprocalDirection[axis];
				KdNode* node1, *node2;
				float rayOrigin = ray.Origin[axis];
				if (rayOrigin < node->SplitPlane)
				{
					node1 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());//node + 1;
					node2 = node1 + 1;//(KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());
				}
				else
				{
					node1 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset()) + 1;
					node2 = node1 - 1;
				}
				if (tplane >= tMax || tplane < 0)
				{
					node = node1;
				}
				else if (tplane <= tMin)
				{
					node = node2;
				}
				else
				{
					node = node1;
				}
			}
			DifferentialGeometry inter;
			int count = node->GetElementCount();
			
			int* elemIds = (int*)((char*)tree.Leaves.Buffer() + node->LeafPtr);
			KdLeafHeader * leaf = (KdLeafHeader*)(elemIds + count);
			for (int i = 0; i < count; i++)
			{
				T*curElem = tree.Elements.Buffer() + elemIds[i];
				float it = FLT_MAX;
				if (tracer.Trace(inter, *curElem, elemIds[i], traceRay, rayDiff, it))
				{
					if (pred)
						return true;
					// bias t
					it += system->GeometryBias*Vec3::Dot(inter.Normal, ray.Direction);
					if (it<tMax)
					{
						tMax = it;
						rs = inter;
						t = it;
						intersected = true;
					}
				}
			}
			// Locate next node
			int face;
			RayBBoxExit(leaf->Bounds, ray, tMin, tMin, face);
			tMin -= system->GeometryBias;
			int nextPtr = leaf->Ropes[face];
			if (nextPtr == -1)
				break;
			node = (KdNode*)((char*)tree.Nodes.Buffer() + nextPtr);
		}
		return intersected;
	}


	template <typename T, typename CreateLeafSpheresFunction>
	void ComputeKdTreeBoundingSpheres(KdTree<T> &tree, int depth, Array<Vec4, true> &buffer, CreateLeafSpheresFunction & cleaf)
	{
#define ADD_SPHERE \
			Vec3::Add(position, curBox.Min, curBox.Max); \
			Vec3::Scale(position, position, 0.5f); \
			sphere.x = position.x; sphere.y = position.y; sphere.z = position.z; \
			Vec3::Subtract(range, curBox.Max, curBox.Min); \
			sphere.w = range.Length() * 0.5f; \
			buffer.Add(sphere);
#ifdef DEBUG
		if (depth > 5)
			throw "Depth too large";
#endif
		int queue1[32], queue2[32];
		int *toTraverse = queue1, *nextQueue = queue2;
		int qPtr = 1, nPtr = 0;
		Vec3 position, range;
		Vec4 sphere;
		BBox boxes1[32], boxes2[32];
		BBox *boxes = boxes1, *nextBoxes = boxes2;
		int bboxCount = 0;
		toTraverse[0] = 0;
		boxes[0] = tree.Bounds;
		while (qPtr)
		{
			depth --;
			for (int i = 0; i<qPtr; i++)
			{
				int nodeId = toTraverse[i];
				BBox curBox = boxes[i];
				if (tree.Nodes[nodeId].GetIsLeaf())
				{
#ifdef DEBUG
					if (spheres >= bufferSize)
						throw "Output buffer overflow";
#endif
					cleaf(curBox, nodeId, depth);
				}
				else
				{
					if (depth == 0)
					{
#ifdef DEBUG
						if (spheres >= bufferSize)
							throw "Output buffer overflow";
#endif
						ADD_SPHERE
					}
					else
					{
#ifdef DEBUG
						if (nPtr >= 32)
							throw "Buffer overflow";
#endif
						if (tree.Nodes[nodeId+1].GetElementCount())
						{
							BBox box;
							box = curBox;
							box.Max[tree.Nodes[nodeId].GetSplitDimension()] = tree.Nodes[nodeId].SplitPlane;
							
							nextBoxes[nPtr] = box;
							nextQueue[nPtr] = nodeId+1;
							nPtr++;
						}
						int subId = tree.Nodes[nodeId].GetOffset()>>3;
						if (tree.Nodes[subId].GetElementCount())
						{
							BBox box;
							box = curBox;
							box.Min[tree.Nodes[nodeId].GetSplitDimension()] = tree.Nodes[nodeId].SplitPlane;
							
							nextBoxes[nPtr] = box;
							nextQueue[nPtr] = subId;
							nPtr++;
						}
					}
					
				}
			}
			int * tmpQueue = nextQueue;
			nextQueue = toTraverse;
			toTraverse = tmpQueue;
			BBox * tmpBox = boxes;
			boxes = nextBoxes;
			nextBoxes = tmpBox;
			qPtr = nPtr;
			nPtr = 0;
		}
	}

}

#endif